# coding=utf-8
import time

import RPi.GPIO as GPIO

from config import Config
from photoelectricsensor import PhotoElectricSensor

'''
步进电机控制类
'''


class Stepper:
    def __init__(self, pulGpio, dirGpio, reverse, setted_cycle, switch_low=None, switch_middle=None, switch_high=None):
        self.pulGpio = int(pulGpio)
        self.dirGpio = int(dirGpio)
        self.reverse = reverse
        self.setted_cycle = setted_cycle
        self.switch_low = switch_low
        self.switch_middle = switch_middle
        self.switch_high = switch_high
        GPIO.setwarnings(False)
        GPIO.setmode(GPIO.BCM)
        GPIO.setup(self.pulGpio, GPIO.OUT)
        GPIO.setup(self.dirGpio, GPIO.OUT)
        self.current_offset=0
        self.current_position=0


    #步长计数方式驱动电机。其中dirction 控制电机正反转，1为正转，-1为反转
    def run_motor(self, stepcount, direction, cycle):
        if direction * self.reverse == 1:
           GPIO.output(self.dirGpio, True)
        else:
           GPIO.output(self.dirGpio, False)
        while stepcount > 0:
              GPIO.output(self.pulGpio, True)
              time.sleep(cycle/2)
              GPIO.output(self.pulGpio, False)
              time.sleep(cycle/2)
              stepcount -= 1


    #相对起点位移(步长计数)方式驱动电机
    def run_motor_offset(self,target_offset):
         difference=target_offset - self.current_offset
         if difference > 0:
            self.run_motor(difference,1, 0.001)
         elif difference < 0:
            difference = difference * (-1)
            self.run_motor(difference,-1,0.001)
         else: pass
         self.current_offset = target_offset


  

     #指定位置方式驱动电机。其中target_posion=0代表起点，1代表中点，2代表终点
    def run_motor_position(self, target_position):
         
         if target_position == 0:
             while (self.switch_low).checkObstacle() != 1:
               self.run_motor(50, -1, 0.001)
             self.current_position=0
         elif target_position == 2:
             while (self.switch_high).checkObstacle() != 1:
               self.run_motor(50, 1, 0.001)
             self.current_position=2
         else:
             if self.current_position == 0:
               while (self.switch_middle).checkObstacle() != 1:
                  self.run_motor(50, 1, 0.001)
               self.run_motor(2100,1,0.001) # 升降机继续提升
             elif self.current_position == 2:
               while (self.switch_middle).checkObstacle() != 1:
                  self.run_motor(50, -1, 0.001)
               self.run_motor(80,1,0.001) # 升降机提升
             else:
               pass
             self.current_position=1



    #电机驱动丝杆位置归零
    def motor_to_origin(self):
        while (self.switch_low).checkObstacle() != 1:
            self.run_motor(50, -1, 0.001)
        self.current_position=0
        self.current_offset=0





#同时运行两台电机
class Twosteppers:
    def __init__(self, pulGpio1, dirGpio1, pulGpio2, dirGpio2, reverse, setted_cycle):
        self.pulGpio1 = int(pulGpio1)
        self.dirGpio1 = int(dirGpio1)
        self.pulGpio2 = int(pulGpio2)
        self.dirGpio2 = int(dirGpio2)
        self.reverse = reverse
        self.setted_cycle = setted_cycle
        GPIO.setwarnings(False)
        GPIO.setmode(GPIO.BCM)
        GPIO.setup(self.pulGpio1, GPIO.OUT)
        GPIO.setup(self.dirGpio1, GPIO.OUT)
        GPIO.setup(self.pulGpio2, GPIO.OUT)
        GPIO.setup(self.dirGpio2, GPIO.OUT)



    #步长计数方式驱动电机。其中dirction 控制电机正反转，1为正转，-1为反转
    def run_motors(self, stepcount, direction, cycle):
        if direction * self.reverse == 1:
           GPIO.output(self.dirGpio1, True)
           GPIO.output(self.dirGpio2, True)
        else:
           GPIO.output(self.dirGpio1, False)
           GPIO.output(self.dirGpio2, False)
        while stepcount > 0:
              GPIO.output(self.pulGpio1, True)
              GPIO.output(self.pulGpio2, True)
              time.sleep(cycle/2)
              GPIO.output(self.pulGpio1, False)
              GPIO.output(self.pulGpio2, False)
              time.sleep(cycle/2)
              stepcount -= 1


         
if __name__ == "__main__":
   pass
        
